/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"
#include <stdio.h>


namespace cv {

static void
calcMinEigenVal( const Mat& _cov, Mat& _dst ) {
	int i, j;
	Size size = _cov.size();
#if CV_SSE
	volatile bool simd = checkHardwareSupport(CV_CPU_SSE);
#endif

	if ( _cov.isContinuous() && _dst.isContinuous() ) {
		size.width *= size.height;
		size.height = 1;
	}

	for ( i = 0; i < size.height; i++ ) {
		const float* cov = (const float*)(_cov.data + _cov.step * i);
		float* dst = (float*)(_dst.data + _dst.step * i);
		j = 0;
#if CV_SSE
		if ( simd ) {
			__m128 half = _mm_set1_ps(0.5f);
			for ( ; j <= size.width - 5; j += 4 ) {
				__m128 t0 = _mm_loadu_ps(cov + j * 3); // a0 b0 c0 x
				__m128 t1 = _mm_loadu_ps(cov + j * 3 + 3); // a1 b1 c1 x
				__m128 t2 = _mm_loadu_ps(cov + j * 3 + 6); // a2 b2 c2 x
				__m128 t3 = _mm_loadu_ps(cov + j * 3 + 9); // a3 b3 c3 x
				__m128 a, b, c, t;
				t = _mm_unpacklo_ps(t0, t1); // a0 a1 b0 b1
				c = _mm_unpackhi_ps(t0, t1); // c0 c1 x x
				b = _mm_unpacklo_ps(t2, t3); // a2 a3 b2 b3
				c = _mm_movelh_ps(c, _mm_unpackhi_ps(t2, t3)); // c0 c1 c2 c3
				a = _mm_movelh_ps(t, b);
				b = _mm_movehl_ps(b, t);
				a = _mm_mul_ps(a, half);
				c = _mm_mul_ps(c, half);
				t = _mm_sub_ps(a, c);
				t = _mm_add_ps(_mm_mul_ps(t, t), _mm_mul_ps(b, b));
				a = _mm_sub_ps(_mm_add_ps(a, c), _mm_sqrt_ps(t));
				_mm_storeu_ps(dst + j, a);
			}
		}
#endif
		for ( ; j < size.width; j++ ) {
			double a = cov[j*3] * 0.5;
			double b = cov[j*3+1];
			double c = cov[j*3+2] * 0.5;
			dst[j] = (float)((a + c) - std::sqrt((a - c) * (a - c) + b * b));
		}
	}
}


static void
calcHarris( const Mat& _cov, Mat& _dst, double k ) {
	int i, j;
	Size size = _cov.size();
#if CV_SSE
	volatile bool simd = checkHardwareSupport(CV_CPU_SSE);
#endif

	if ( _cov.isContinuous() && _dst.isContinuous() ) {
		size.width *= size.height;
		size.height = 1;
	}

	for ( i = 0; i < size.height; i++ ) {
		const float* cov = (const float*)(_cov.data + _cov.step * i);
		float* dst = (float*)(_dst.data + _dst.step * i);
		j = 0;

#if CV_SSE
		if ( simd ) {
			__m128 k4 = _mm_set1_ps((float)k);
			for ( ; j <= size.width - 5; j += 4 ) {
				__m128 t0 = _mm_loadu_ps(cov + j * 3); // a0 b0 c0 x
				__m128 t1 = _mm_loadu_ps(cov + j * 3 + 3); // a1 b1 c1 x
				__m128 t2 = _mm_loadu_ps(cov + j * 3 + 6); // a2 b2 c2 x
				__m128 t3 = _mm_loadu_ps(cov + j * 3 + 9); // a3 b3 c3 x
				__m128 a, b, c, t;
				t = _mm_unpacklo_ps(t0, t1); // a0 a1 b0 b1
				c = _mm_unpackhi_ps(t0, t1); // c0 c1 x x
				b = _mm_unpacklo_ps(t2, t3); // a2 a3 b2 b3
				c = _mm_movelh_ps(c, _mm_unpackhi_ps(t2, t3)); // c0 c1 c2 c3
				a = _mm_movelh_ps(t, b);
				b = _mm_movehl_ps(b, t);
				t = _mm_add_ps(a, c);
				a = _mm_sub_ps(_mm_mul_ps(a, c), _mm_mul_ps(b, b));
				t = _mm_mul_ps(_mm_mul_ps(t, t), k4);
				a = _mm_sub_ps(a, t);
				_mm_storeu_ps(dst + j, a);
			}
		}
#endif

		for ( ; j < size.width; j++ ) {
			double a = cov[j*3];
			double b = cov[j*3+1];
			double c = cov[j*3+2];
			dst[j] = (float)(a * c - b * b - k * (a + c) * (a + c));
		}
	}
}


static void
calcEigenValsVecs( const Mat& _cov, Mat& _dst ) {
	int i, j;
	Size size = _cov.size();
	if ( _cov.isContinuous() && _dst.isContinuous() ) {
		size.width *= size.height;
		size.height = 1;
	}

	for ( i = 0; i < size.height; i++ ) {
		const float* cov = (const float*)(_cov.data + _cov.step * i);
		float* dst = (float*)(_dst.data + _dst.step * i);

		for ( j = 0; j < size.width; j++ ) {
			double a = cov[j*3];
			double b = cov[j*3+1];
			double c = cov[j*3+2];

			double u = (a + c) * 0.5;
			double v = std::sqrt((a - c) * (a - c) * 0.25 + b * b);
			double l1 = u + v;
			double l2 = u - v;

			double x = b;
			double y = l1 - a;
			double e = fabs(x);

			if ( e + fabs(y) < 1e-4 ) {
				y = b;
				x = l1 - c;
				e = fabs(x);
				if ( e + fabs(y) < 1e-4 ) {
					e = 1. / (e + fabs(y) + FLT_EPSILON);
					x *= e, y *= e;
				}
			}

			double d = 1. / std::sqrt(x * x + y * y + DBL_EPSILON);
			dst[6*j] = (float)l1;
			dst[6*j + 2] = (float)(x * d);
			dst[6*j + 3] = (float)(y * d);

			x = b;
			y = l2 - a;
			e = fabs(x);

			if ( e + fabs(y) < 1e-4 ) {
				y = b;
				x = l2 - c;
				e = fabs(x);
				if ( e + fabs(y) < 1e-4 ) {
					e = 1. / (e + fabs(y) + FLT_EPSILON);
					x *= e, y *= e;
				}
			}

			d = 1. / std::sqrt(x * x + y * y + DBL_EPSILON);
			dst[6*j + 1] = (float)l2;
			dst[6*j + 4] = (float)(x * d);
			dst[6*j + 5] = (float)(y * d);
		}
	}
}


enum { MINEIGENVAL = 0, HARRIS = 1, EIGENVALSVECS = 2 };


static void
cornerEigenValsVecs( const Mat& src, Mat& eigenv, int block_size,
					 int aperture_size, int op_type, double k = 0.,
					 int borderType = BORDER_DEFAULT ) {
	int depth = src.depth();
	double scale = (double)(1 << ((aperture_size > 0 ? aperture_size : 3) - 1)) * block_size;
	if ( aperture_size < 0 ) {
		scale *= 2.;
	}
	if ( depth == CV_8U ) {
		scale *= 255.;
	}
	scale = 1. / scale;

	CV_Assert( src.type() == CV_8UC1 || src.type() == CV_32FC1 );

	Mat Dx, Dy;
	if ( aperture_size > 0 ) {
		Sobel( src, Dx, CV_32F, 1, 0, aperture_size, scale, 0, borderType );
		Sobel( src, Dy, CV_32F, 0, 1, aperture_size, scale, 0, borderType );
	} else {
		Scharr( src, Dx, CV_32F, 1, 0, scale, 0, borderType );
		Scharr( src, Dy, CV_32F, 0, 1, scale, 0, borderType );
	}

	Size size = src.size();
	Mat cov( size, CV_32FC3 );
	int i, j;

	for ( i = 0; i < size.height; i++ ) {
		float* cov_data = (float*)(cov.data + i * cov.step);
		const float* dxdata = (const float*)(Dx.data + i * Dx.step);
		const float* dydata = (const float*)(Dy.data + i * Dy.step);

		for ( j = 0; j < size.width; j++ ) {
			float dx = dxdata[j];
			float dy = dydata[j];

			cov_data[j*3] = dx * dx;
			cov_data[j*3+1] = dx * dy;
			cov_data[j*3+2] = dy * dy;
		}
	}

	boxFilter(cov, cov, cov.depth(), Size(block_size, block_size),
			  Point(-1, -1), false, borderType );

	if ( op_type == MINEIGENVAL ) {
		calcMinEigenVal( cov, eigenv );
	} else if ( op_type == HARRIS ) {
		calcHarris( cov, eigenv, k );
	} else if ( op_type == EIGENVALSVECS ) {
		calcEigenValsVecs( cov, eigenv );
	}
}


void cornerMinEigenVal( const Mat& src, Mat& dst, int blockSize, int ksize, int borderType ) {
	dst.create( src.size(), CV_32F );
	cornerEigenValsVecs( src, dst, blockSize, ksize, MINEIGENVAL, 0, borderType );
}


void cornerHarris( const Mat& src, Mat& dst, int blockSize, int ksize, double k, int borderType ) {
	dst.create( src.size(), CV_32F );
	cornerEigenValsVecs( src, dst, blockSize, ksize, HARRIS, k, borderType );
}


void cornerEigenValsAndVecs( const Mat& src, Mat& dst, int blockSize, int ksize, int borderType ) {
	if ( dst.rows != src.rows || dst.cols * dst.channels() != src.cols * 6 || dst.depth() != CV_32F ) {
		dst.create( src.size(), CV_32FC(6) );
	}
	cornerEigenValsVecs( src, dst, blockSize, ksize, EIGENVALSVECS, 0, borderType );
}


void preCornerDetect( const Mat& src, Mat& dst, int ksize, int borderType ) {
	Mat Dx, Dy, D2x, D2y, Dxy;

	CV_Assert( src.type() == CV_8UC1 || src.type() == CV_32FC1 );
	dst.create( src.size(), CV_32F );

	Sobel( src, Dx, CV_32F, 1, 0, ksize, 1, 0, borderType );
	Sobel( src, Dy, CV_32F, 0, 1, ksize, 1, 0, borderType );
	Sobel( src, D2x, CV_32F, 2, 0, ksize, 1, 0, borderType );
	Sobel( src, D2y, CV_32F, 0, 2, ksize, 1, 0, borderType );
	Sobel( src, Dxy, CV_32F, 1, 1, ksize, 1, 0, borderType );

	double factor = 1 << (ksize - 1);
	if ( src.depth() == CV_8U ) {
		factor *= 255;
	}
	factor = 1. / (factor * factor * factor);

	Size size = src.size();
	int i, j;
	for ( i = 0; i < size.height; i++ ) {
		float* dstdata = (float*)(dst.data + i * dst.step);
		const float* dxdata = (const float*)(Dx.data + i * Dx.step);
		const float* dydata = (const float*)(Dy.data + i * Dy.step);
		const float* d2xdata = (const float*)(D2x.data + i * D2x.step);
		const float* d2ydata = (const float*)(D2y.data + i * D2y.step);
		const float* dxydata = (const float*)(Dxy.data + i * Dxy.step);

		for ( j = 0; j < size.width; j++ ) {
			double dx = dxdata[j];
			double dy = dydata[j];
			dstdata[j] = (float)(factor * (dx * dx * d2ydata[j] + dy * dy * d2xdata[j] - 2 * dx * dy * dxydata[j]));
		}
	}
}


}

CV_IMPL void
cvCornerMinEigenVal( const CvArr* srcarr, CvArr* dstarr,
					 int block_size, int aperture_size ) {
	cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);

	CV_Assert( src.size() == dst.size() && dst.type() == CV_32FC1 );
	cv::cornerMinEigenVal( src, dst, block_size, aperture_size, cv::BORDER_REPLICATE );
}

CV_IMPL void
cvCornerHarris( const CvArr* srcarr, CvArr* dstarr,
				int block_size, int aperture_size, double k ) {
	cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);

	CV_Assert( src.size() == dst.size() && dst.type() == CV_32FC1 );
	cv::cornerHarris( src, dst, block_size, aperture_size, k, cv::BORDER_REPLICATE );
}


CV_IMPL void
cvCornerEigenValsAndVecs( const void* srcarr, void* dstarr,
						  int block_size, int aperture_size ) {
	cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);

	CV_Assert( src.rows == dst.rows && src.cols * 6 == dst.cols * dst.channels() && dst.depth() == CV_32F );
	cv::cornerEigenValsAndVecs( src, dst, block_size, aperture_size, cv::BORDER_REPLICATE );
}


CV_IMPL void
cvPreCornerDetect( const void* srcarr, void* dstarr, int aperture_size ) {
	cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr);

	CV_Assert( src.size() == dst.size() && dst.type() == CV_32FC1 );
	cv::preCornerDetect( src, dst, aperture_size, cv::BORDER_REPLICATE );
}

/* End of file */
